
#ifndef _AFFINEDEF_H_
#define _AFFINEDEF_H_

#include "mesh.h"

class AffineDeformer {
   
public:
   AffineDeformer(GSkeleton *skelO, GSkeleton *skelD) {
      
      cout<<"AffineDeformer begin ...."<<endl;
      si = skelO;
      sd = skelD;
      
      /// initialization
      ubones.resize(skelO->nBones(), Vector3(0,0,0));
      tvectors0.resize(skelO->nBones(), Vector3(0,0,0));
      tvectors1.resize(skelO->nBones(), Vector3(0,0,0));
      tscalar0.resize(skelO->nBones(),0);
      tscalar1.resize(skelO->nBones(),0);
      tlvectors0.resize(skelO->nBones(), Vector3(0,0,0));
      tlvectors1.resize(skelO->nBones(), Vector3(0,0,0));
      
      old_angle.clear();
      old_angle.resize(skelO->nBones());
      for (int i=0; i<old_angle.size(); ++i) {
         vector<Number> array(skelO->nBones(), 0.0);
         old_angle[i] = array;
      }
      
      /// parameters computation for each bone
      for (unsigned int ii=0; ii<skelO->nBones(); ++ii) {
         int i = skelO->orderList[ii];
         //~ int i = ii;
         assert(i != -1);
         
         Vector3 oldu = ubones[i];
         comprotaround = true;
         getRotationRep(i); // compute the ubones[i]
         
         if (comprotaround) {
            
            Vector3 urot(0,0,0);
            urot = getRotationAroundBoneRep(i);
            if (i==5 or i==15) {
               urot = Vector3(0,0,0);
            }
            if (i==8 or i==10) {
               urot = urot/(sqrt(urot*urot));
            }
            if (i==16 or i==18) {
               urot = urot/(sqrt(urot*urot));
            }
            
            
            //~ if (abs(1.0-ubones[i]*urot)<1e-8) {
               //~ Vector3 unitu = (ubones[i]+urot+cross_product(ubones[i],urot));
               //~ Number nunitu = sqrt(unitu*unitu);
               //~ if (nunitu < 1e-8)
                  //~ ubones[i] = oldu;
               //~ else {
                  //~ unitu = unitu/nunitu;
                  //~ ubones[i] = unitu*(SGN(oldu*unitu)*1e8);
               //~ }
            //~ }
            //~ else {
               //~ ubones[i] = (ubones[i]+urot+cross_product(ubones[i],urot))/(1-ubones[i]*urot);
            //~ }
            
            //~ if (i==8 or i==10) urot = Vector3(0,0,0);
            
            /// another method
            Number cosbonei = 1.0/sqrt(1+ubones[i]*ubones[i]);
            Vector3 vecbonei = ubones[i]*cosbonei;
            
            Number cosroti = 1.0/sqrt(1+urot*urot);
            Vector3 vecroti = urot*cosroti;
            
            Number cosprod = cosbonei*cosroti - vecbonei*vecroti;
            Vector3 vecprod = vecroti*cosbonei + vecbonei*cosroti + cross_product(vecbonei,vecroti);
            if (cosprod<0) { 
               cosprod = -cosprod;
               vecprod = vecprod*(-1);
            }
            
            if (cosprod < 0.01)  cosprod = 0.01;
            ubones[i] = vecprod/cosprod;
         }
         else {
            //cout<<"else comprotaround = true"<<endl;
            comprotaround = true;
         }
      }
      
      ubones[5] = Vector3(0,1,0);
      ubones[15] = Vector3(0,-1,0);
      //~ ubones[8] = Vector3(-1,1,1);
      //~ ubones[10] = Vector3(-1,1,1);
      
      /// second phase
      
      for (unsigned int i=0; i<si->nBones(); ++i) {
         const Bone &b = si->bones[i];   // initial bone
         const Point3 &p0 = si->getJointPos(b.first); 
         const Point3 &p1 = si->getJointPos(b.second);
         const Point3 &q0 = sd->getJointPos(b.first); 
         const Point3 &q1 = sd->getJointPos(b.second);
         
         if (i==8) {
            Vector3 vecBone(p1,p0);
            cout<<"####################################### vecBone: "<<vecBone<<endl;
            vecBone = (vecBone/sqrt(vecBone*vecBone));
            Number x = -vecBone[1]/(1+vecBone[2]+vecBone[0]);
            Number y = (1+vecBone[2]-vecBone[0])/(1+vecBone[2]+vecBone[0]);
            Number z = -x;
            ubones[i] = Vector3(x,y,z);
            
            cout<<"####################################### ubones[i]: "<<ubones[i]<<endl;
         }
         
         if (i==10) ubones[10] = ubones[8];
         
         Number nubone2 = ubones[i]*ubones[i];
         Number denom = 1.0/sqrt(1.0 + nubone2);
         Vector3 utemp = ubones[i]*denom;
         {
         Vector3 rv = rotate(p0-Point3(0,0,0), ubones[i]);
         Vector3 t0 = q0 - Point3(rv[0],rv[1],rv[2]);
         tscalar0[i] = -1.0*(t0*utemp);
         tlvectors0[i] = t0;
         tvectors0[i] = (t0*denom - cross_product(utemp,t0));
         }
         
         {
         Vector3 rv = rotate(p1-Point3(0,0,0), ubones[i]);
         Vector3 t1 = q1 - Point3(rv[0],rv[1],rv[2]);
         tscalar1[i] = -1.0*(t1*utemp);
         tlvectors1[i] = t1;
         tvectors1[i] = (t1*denom - cross_product(utemp,t1));
         }
      }
      
      cout<<"AffineDeformer .... end"<<endl;
   }
   
   void execute(Mesh *mesh) {
      
      cout<<"deformation.... begin"<<endl;
      // project deformation on the mesh
      /// on vertices
      unsigned int ii=0, iv=0;
      Vertex_iterator vi = mesh->vertices_begin();
      while (vi != mesh->vertices_end()) {
         vector<Vector3> vt(vi->weights.size(), Vector3(0,0,0));
         vector<Number> sc(vi->weights.size(), 0);
         Point3 po = vi->point();
         Vector3 no = vi->normal();
         
         Number sumweights = 0;
         
         Number s1 = 0;
         Vector3 t1(0,0,0);
         Number qs1 = 0;
         Vector3 qv1(0,0,0);
         
         Vector3 vtl(0,0,0);
         
         Vector3 u1(0,0,0);
         Vector3 vmod(0,0,0); // position
         Vector3 nmod(0,0,0); // normal
         for (unsigned int i=0; i<vi->weights.size(); ++i) {
            if (vi->weights[i] == 0) continue;
            
            vtl = (tlvectors0[i])*(vi->ts[i]) + (tlvectors1[i])*(1.0-vi->ts[i]);
            vt[i] = (tvectors0[i])*(vi->ts[i]) + (tvectors1[i])*(1.0-vi->ts[i]);
            sc[i] = (tscalar0[i])*(vi->ts[i])  + (tscalar1[i])*(1.0-vi->ts[i]);
            
            /// using ?
            {
            Vector3 rv = rotate(po-Point3(0,0,0), ubones[i]);
            vmod = vmod + (rv + vtl)*vi->weights[i];
            }
            {
            Vector3 rv = rotate(no, ubones[i]);
            nmod = nmod + (rv)*vi->weights[i];
            }
            
            /// using quat
            Number cosi = (sqrt(1.0/(1.0+(ubones[i])*(ubones[i]))));
            Vector3 vecparti = ubones[i]*cosi;
            
            if ((qs1*cosi + qv1*vecparti)<0) {
               cosi = -cosi;
               vecparti = -vecparti;
               vt[i] = vt[i]*(-1.0);
               sc[i] = sc[i]*(-1.0);
               //~ cout<<"changed ..."<<endl;
            }
            s1 += sc[i]*vi->weights[i];
            t1 = t1 + vt[i]*vi->weights[i];
            qs1 += cosi*vi->weights[i];
            qv1 = qv1 + (vecparti)*vi->weights[i];
            
            sumweights += vi->weights[i];
         }
         
         vmod = vmod/sumweights;
         nmod = nmod/sumweights;
         s1 /= sumweights;
         t1 = t1/sumweights;
         qs1 /= sumweights;
         qv1 = qv1/sumweights;
         
         Number normaq = sqrt(qs1*qs1 + qv1*qv1);
         assert(normaq != 0);
         qs1 = qs1/normaq;
         qv1 = qv1/normaq;
         s1 = s1/normaq;
         t1 = t1/normaq;
         
         Vector3 tv = t1*qs1 - qv1*s1 + cross_product(qv1, t1);
         //~ Vector3 tv = t1;
         
         /// 1) Using the u rotation representation
         // Rotate the vertex position
         Number ux = qv1[0]*(1.0/qs1);
         Number uy = qv1[1]*(1.0/qs1);
         Number uz = qv1[2]*(1.0/qs1);
         Vector3 u(ux, uy, uz);
         Vector3 rv = rotate(po-Point3(0,0,0), u);
         Point3 &p = vi->point();
         p = Point3(tv[0]+rv[0], tv[1]+rv[1], tv[2]+rv[2]);
         // Rotate the normal vector
         Vector3 &n = vi->normal();
         n = rotate(no, u);
         
         /// 2) Using the ??? method
         //~ // Update the vertex position
         //~ Point3 &p = vi->point();
         //~ p = Point3(vmod[0],vmod[1],vmod[2]);
         //~ // Upate the normal vector
         //~ Vector3 &n = vi->normal();
         //~ n = Vector3(nmod[0], nmod[1], nmod[2]);
         
         /// 3) Using the dual quaternion method
         //~ typedef Quaternion<Number> Quat;
         //~ Quat Q(qs1, qv1[0],qv1[1],qv1[2]);
         //~ // Rotate the vertex position
         //~ //const Point3 &Po = ipositions[ii];
         //~ const Point3 &Po = vi->point();
         //~ Quat qPo(Po[0],Po[1],Po[2]);
         //~ Quat qp = Q*qPo*conjugate(Q);
         //~ Point3 &P = vi->point();
         //~ P = Point3(qp[1],qp[2],qp[3]) + tv; // the deformed vertex position
         //~ // Rotate the normal vector
         //~ //const Vector3 &No = inormals[ii];
         //~ const Vector3 &No = vi->normal();
         //~ Quat qNo(No[0],No[1],No[2]);
         //~ Quat qn = Q*qNo*conjugate(Q);
         //~ Vector3 &n = vi->normal();
         //~ n = Vector3(qn[1], qn[2], qn[3]);
         
         /// ////////////////////
         ++vi; ++ii; iv+=3;
      }

      cout<<" ... deformation done .... "<<endl;
   }
   
   Vector3 rotate(const Vector3 &v, const Vector3 &u) {
      
      /// Perform the rotation of v
      Number vxu[3], rv[3], factor;
      vxu[0] = v[1]*u[2]-v[2]*u[1];
      vxu[1] = v[2]*u[0]-v[0]*u[2];
      vxu[2] = v[0]*u[1]-v[1]*u[0];
      
      factor = -2.0/(1.0 + u[0]*u[0] + u[1]*u[1] + u[2]*u[2]);
      rv[0] = u[1]*vxu[2] - u[2]*vxu[1] + vxu[0];
      rv[1] = u[2]*vxu[0] - u[0]*vxu[2] + vxu[1];
      rv[2] = u[0]*vxu[1] - u[1]*vxu[0] + vxu[2];
      
      rv[0] = v[0] + factor*rv[0];
      rv[1] = v[1] + factor*rv[1];
      rv[2] = v[2] + factor*rv[2];
      
      return Vector3(rv[0], rv[1], rv[2]);
   }
   
   void getRotationRep(unsigned int i) {
      
      cout<<"getRotationRep() ... begin"<<endl;
      const Bone &bi = si->bones[i];   // initial bone
      const Bone &bd = sd->bones[i];   // deformed bone
      
      Vector3 di = si->getJointPos(bi.second) - si->getJointPos(bi.first); // direction vector of initial bone 
      Vector3 dd = sd->getJointPos(bd.second) - sd->getJointPos(bd.first); // direction vector of deformed bone 
      
      di = di/sqrt(di*di); // normalize
      dd = dd/sqrt(dd*dd); // normalize
      
      Vector3 a = cross_product(di,dd);
      if (a*a < 1.0e-4) { // bone i collinear with the initial bone i
         comprotaround=false; 
         return; 
      }
      
      Vector3 bisectriz = (di+dd)*0.5;
      Number normab = sqrt(bisectriz*bisectriz);
      bisectriz = bisectriz/(normab);
      ubones[i] = bisectriz*(ubones[i]*bisectriz);
      
      Number pdot = di*dd;
      if (pdot >= 1.0) pdot = 1.0; // numerical error
      
      Number normau;
      a =a/sqrt(a*a);
      if(pdot <= -1.0) normau = 1.0e8; // numerical error or rotation of 180 degrees
      else normau = sqrt((1.0-pdot)/(1.0+pdot)); 
      
      ubones[i] = ubones[i] + a*normau;
      cout<<"getRotationRep() ... end"<<endl;
   }
   
   Vector3 getRotationAroundBoneRep(unsigned int i) {
      
      cout<<"getRotationAroundBoneRep() ... begin"<<endl;
      const Bone &bone = si->bones[i];   // initial bone
      Vector3 vbone = si->getJointPos(bone.second) - si->getJointPos(bone.first); // direction vector of initial bone 
      Vector3 vbonerot = sd->getJointPos(bone.second) - sd->getJointPos(bone.first);
      vbone = vbone/sqrt(vbone*vbone); // normalize
      vbonerot = vbonerot/sqrt(vbonerot*vbonerot); // normalize
      
      Number normau = 0;
      Number angle=0, sumangle=0;
      Number sumw=0;
      Number w = 1.0/(Number)si->controller[i].size();
      
      for (unsigned int ii=0; ii<si->controller[i].size(); ++ii) {
         
         cout<<"for ii() ... "<<ii<<endl;
         
         if (si->controller[i][ii] == -1) {
            assert(si->controller[i].size() == 1);
            cout<<" ifff"<<endl;
            return vbone;
         }
            
         cout<<"for iiiiiii() ... "<<si->controller[i][ii]<<" :: "<<endl;
         
         Vector3 di; Vector3 ddorig;
         int j = si->controller[i][ii];
         if (j>=si->bones.size()) {
            cout<<"if"<<endl;
            j = j%si->bones.size();
            di = si->atbones[j];
            ddorig = rotate(di, ubones[j]);
            if (j == 6) {
               cout<<"YYYY: "<<di<<" : "<<ddorig<<" :: "<<ubones[j]<<endl;
            }
         }
         else {
            cout<<"else"<<endl;
            const Bone &bi = si->bones[j];   // initial bone
            const Bone &bd = sd->bones[j];   // deformed bone
            di = si->getJointPos(bi.second) - si->getJointPos(bi.first); // direction vector of initial bone 
            ddorig = sd->getJointPos(bd.second) - sd->getJointPos(bd.first); // direction vector of deformed bone
         }
         
         cout<<"11111111"<<endl;
         
         Vector3 u = ubones[i]*(-1.0);
         Vector3 dd = rotate(ddorig, u);
         
         if (i == 8) {
            cout<<"ZZZZ: "<<di<<" : "<<" : "<<ddorig<<" : "<<rotate(di,ubones[i])<<endl;
         }
         
         Vector3 unidi = di/sqrt(di*di);
         Vector3 unidd = dd/sqrt(dd*dd);
         
         /// 1er metodo
         /*
         Vector3 bisectriz = (unidi+unidd)*0.5;
         Number normab = sqrt(bisectriz*bisectriz);
         
         if (normab != 0) {
            bisectriz = bisectriz*(1.0/normab);
            
            Number pdot = unidi*unidd;
            if (pdot > 1.0) pdot = 1.0;
            if (pdot <-1.0) pdot =-1.0;
            Number angleini = acos(pdot);
            
            Number normav = tan(angleini/2.0);
            Vector3 u0 = cross_product(unidi,unidd);
            if (u0*u0 < 1e-8) { u0=Vector3(0,0,0); cout<<"u0=0"<<endl;}
            else              u0 = (u0/(sqrt(u0*u0)))*normav;
            Vector3 u1 = cross_product(bisectriz,vbone);
            Vector3 u2 = cross_product(u1,bisectriz);
            Number den = u2*vbone;
            if (abs(den)>1e-3) {
               
               normau = (u2*u0)/den;
               angle = 2*atan(normau);
            }
            else {
               cout<<"else else else else else else 11111111111111"<<endl;
               if (abs(u2*u0) < 1e-3) {
                  cout<<"if abs(u2*u0) < 1e-3"<<endl;
                  angle = old_angle[i][j];
                  normau = tan(angle/2.0);
               }
               else {
                  cout<<"else else else else else else else else else "<<endl;
                  if (abs(den)<1e-8) {
                     angle = old_angle[i][j];
                     normau = tan(angle/2.0);
                     //~ normau = SGN(normau)*1e8;
                     cout<<"abs(den)<1e-8 abs(den)<1e-8"<<endl;
                  }
                  else {
                     cout<<"else else 222222222222"<<endl;
                     normau = (u2*u0)/den;
                     angle = 2*atan(normau);
                  }
               }
            }
         }
         else {
            cout<<"else else 3333333333333"<<endl;
            angle = old_angle[i][j];
            normau = tan(angle/2.0);
         }
         */
         
         /// ////// 2do metodo
         di = di - vbone*(di*vbone);
         dd = dd - vbone*(dd*vbone);
         Number ndi = sqrt(di*di);
         Number ndd = sqrt(dd*dd);
         
         if (ndi*ndd < 1e-3) {
            angle = old_angle[i][j];
            //continue;
         }
         else {
            di =di/ndi;
            dd =dd/ndd;
            Vector3 dixdd = cross_product(di,dd);
            if ((dixdd * dixdd) <1.0e-3) {
               angle = old_angle[i][j];
            }
            else {
               Number pdot = di *dd;
               if (pdot > 1.0) pdot = 1.0;
               if (pdot < -1.0) pdot = -1.0;
               angle = acos(pdot);
               if(dixdd *vbone < 0) angle =-angle;
            }
         }
         normau = tan(angle/2.0);
         /// //////////// 
         
         /// ///
         //~ Number dif_angle = angle-old_angle[i][j];
         //~ Number turns_dif;
         //~ if (dif_angle < 0) turns_dif = -1;
         //~ else               turns_dif = 0;
         
         //~ turns_dif += (int)(dif_angle/6.2832);
         //~ angle -= turns_dif*6.2832;
         
         //~ if ((angle-old_angle[i][j])>3.1416)
            //~ angle -= 6.2832;
            
         //~ if (numframe>1) {
            //~ if (abs(angle-old_angle[i][j])>0.5)
               //~ angle = old_angle[i][j];
         //~ }
         
         /// ///
         old_angle[i][j] = angle;
         
         /// verificar
         //~ if (si->manOrAutowab[i]) {
            //~ w = si->wab[i][ii];
         //~ }
         
         sumangle +=  w*angle;
         sumw += w;
      }
      
      if (sumw==0) { sumangle = 0; }
      else sumangle /= sumw;
      
      //~ if (abs(sumangle) > 6.2832) sumangle -= ((int)(sumangle/6.2832))*6.2832;
      //~ if (sumangle > 3.1416)      sumangle -= 6.2832;
      //~ if (sumangle < -3.1416)     sumangle += 6.2832;
      
      sumangle = sumangle/2.0;
      normau = tan(sumangle);
      
      cout<<"getRotationAroundBoneRep() ... end"<<endl;
      return vbone*normau;
   }
   
private:
   
   vector<Vector3>   ul;
   vector<Vector3>   t0l;
   vector<Vector3>   t1l;
   
   /// /////////////
   GSkeleton                 *si;
   GSkeleton                 *sd;
   vector<Vector3>            ubones; // rotation representation for bones
   vector<Vector3>            tvectors0; // translation applied to joints of bones
   vector<Vector3>            tvectors1; // translation applied to joints of bones
   vector<Number>             tscalar0; // 
   vector<Number>             tscalar1; // 
   vector<Vector3>            tlvectors0; // translation applied to joints of bones
   vector<Vector3>            tlvectors1; // translation applied to joints of bones
   vector<vector<Number> >    old_angle;
   
   bool                       comprotaround;
   
   typedef pair<int, int>     Bone;
};

#endif
